15. Radar Measurements

H versus h(x)

The $H$ matrix from the lidar lesson and $h(x)$ equations from the radar lesson are actually accomplishing the same thing; they are both needed to solve $y = z - Hx'$ in the update step.

But for radar, there is no $H$ matrix that will map the state vector $x$ into polar coordinates; instead, you need to calculate the mapping manually to convert from cartesian coordinates to polar coordinates.

Here is the $h$ function that specifies how the predicted position and speed get mapped to the polar coordinates of range, bearing and range rate.

Hence for radar $y = z - Hx'$ becomes $y = z - h(x')$ .

Definition of Radar Variables

• The range, ( $\rho$ ), is the distance to the pedestrian. The range is basically the magnitude of the position vector $\rho$ which can be defined as $\rho = sqrt(p_x^2 + p_y^2)$ .
• $\varphi = atan(p_y / p_x)$ . Note that $\varphi$ is referenced counter-clockwise from the x-axis, so $\varphi$ from the video clip above in that situation would actually be negative.
• The range rate, $\dot{\rho}$ , is the projection of the velocity, $v$ , onto the line, $L$ .

Deriving the Radar Measurement Function

The measurement function is composed of three components that show how the predicted state, $x' = (p_x', p_y', v_x', v_y')^T$ , is mapped into the measurement space, $z = (\rho, \varphi, \dot{\rho})^T$ :

The range, $\rho$ , is the distance to the pedestrian which can be defined as:

$\varphi$ is the angle between $\rho$ and the $x$ direction and can be defined as:

There are two ways to do the range rate $\dot{\rho(t)}$ derivation:

Generally we can explicitly describe the range, $\rho$ , as a function of time:

The range rate, $\dot{\rho(t)}$ , is defined as time rate of change of the range, $\rho$ , and it can be described as the time derivative of $\rho$ :

$\dot{\rho} = \frac{\partial \rho(t)}{\partial t} = \frac{ \partial}{\partial t}\sqrt{p_x(t)^2 + p_y(t)^2} = \frac{1}{2 \sqrt{p_x(t)^2 + p_y(t)^2}} (\frac{ \partial}{\partial t}p_x(t)^2 + \frac{ \partial}{\partial t}p_y(t)^2)$

$=\frac{1}{2 \sqrt{p_x(t)^2 + p_y(t)^2}} (2 p_x(t) \frac{ \partial}{\partial t} p_x(t) + 2 p_y(t) \frac{ \partial}{\partial t} p_y(t))$

$\frac{ \partial}{\partial t} p_x(t)$ is nothing else than $v_x(t)$ , similarly $\frac{ \partial}{\partial t} p_y(t)$ is $v_y(t)$ . So we have:

$\dot{\rho} = \frac{\partial \rho(t)}{\partial t} = \frac{1}{2 \sqrt{p_x(t)^2 + p_y(t)^2}} (2 p_x(t) v_x(t) + 2 p_y(t) v_y(t)) = \frac{2( p_x(t) v_x(t) + p_y(t) v_y(t))}{2 \sqrt{p_x(t)^2 + p_y(t)^2}}$

$=\frac{p_x(t) v_x(t) + p_y(t) v_y(t)}{ \sqrt{p_x(t)^2 + p_y(t)^2}}$

For simplicity we just use the following notation:

The range rate, $\dot{\rho}$ , can be seen as a scalar projection of the velocity vector, $\vec{v}$ , onto $\vec{\rho}$ . Both $\vec{\rho}$ and $\vec{v}$ are 2D vectors defined as:

The scalar projection of the velocity vector $\vec{v}$ onto $\vec{\rho}$ is defined as:

where $\lvert \vec{\rho} \rvert$ is the length of $\vec{\rho}$ . In our case it is actually the range, so $\rho = \lvert \vec{\rho} \rvert$ .

The Next Quiz

$h$ is a nonlinear function. In the next quiz I would like to check your intuition about what that means.